﻿/*
 * This file is part of MonoSettlers.
 *
 * Copyright (C) 2010-2011 Christoph Husse
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors: 
 *      # Christoph Husse
 * 
 * Also checkout our homepage: http://opensettlers.codeplex.com/
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MonoSettlers
{

    /// <summary>
    /// Implementation of paper http://webdocs.cs.ualberta.ca/~silver/David_Silver/Publications_files/coop-path-AIWisdom.pdf.
    /// 
    /// If you didn't read the paper mentioned before, and probably even more about pathfinding, 
    /// you will almost have no chance, except you are a genius ;), to understand what is happening 
    /// in this class and its subclasses. Its just the most complex and essential part of the entire game.
    /// 
    /// My advice, don't touch it, don't look at it, except you are really dedicated and seriously
    /// want to improve the pathfinding engine or fix known bugs etc. BTW by doing this you would 
    /// really do ME a favour and also gain a good reputation ;), but try don't wasting your time 
    /// and effort on it, the game has enough more important and way easier hotspots!
    /// 
    /// TODO: What is missing:
    /// 
    /// * Follow another movable (this is important for soldiers; without it they can't patrole or even fight each other)
    /// * Preventing idles from walking through each other (the feature is already there it's just bugged, and I don't know why; 
    ///              seems to me like they are doing their motion one aligned cycle too late; but shifting it does not solve it, 
    ///              since then the planned cooperation gets srewed up and they are again running through each other but now
    ///              by a different cause)
    /// * Batch planning (we don't seriously want to calculate 100 paths per mouse click xD, and I guess the CPU silently agrees )
    /// 
    /// The following are tough ones I guess; devil is in details:
    /// 
    /// # Honour different sizes and speeds of movables
    /// # Using hierarchical A-Star for backward search. Without it we will definitely run into trouble when it comes to more than
    ///   1000 movables on a map, and this is almost always the case for a usual game.
    ///
    /// I didn't make much comments here since this is not something you can understand by reading them anyway.
    /// Everyone seriously interested will probably have to go the same road I did, understand it from scratch!
    /// </summary>
    internal class CooperativeAStar
    {
        private static readonly Double SQRT_2 = Math.Sqrt(2);
        private SortedDictionary<PathNodeKey, Object> m_ClosedSet = new SortedDictionary<PathNodeKey, Object>(PathNodeKey.Comparer);
        private SortedDictionary<PathNodeKey, Object> m_OpenSet = new SortedDictionary<PathNodeKey, Object>(PathNodeKey.Comparer);
        private PriorityQueue<PathNodeKey> m_OrderedOpenSet = new PriorityQueue<PathNodeKey>();
        private CameFromMap m_CameFrom = new CameFromMap();
        private Double m_PauseCosts = 0.1;
        private IAStarSolver<TerrainCell> m_BackwardAStar;
        private readonly SpaceTimeMap m_Reservation = new SpaceTimeMap();
        private PathNodeKey[,] m_IdleMap;

        public const Double MIN_PAUSE_COSTS = 0.1;

        public Int64 CurrentCycle { get; private set; }

        /// <summary>
        /// Pause costs decide whether the algorithm is likely to not move a unit at all if
        /// there is an obstacle (low pause costs) or find a way round (high pause costs).
        /// Note that a unit is only pausable, if it interacts with reserved cells, meaning
        /// other moving units. It won't pause for a static obstacles.
        /// </summary>
        public Double PauseCosts
        {
            get { return m_PauseCosts; }
            set
            {
                if (value <= MIN_PAUSE_COSTS)
                    throw new ArgumentOutOfRangeException();

                m_PauseCosts = value;
            }
        }
        public Int64 CycleResolution { get; private set; }


        public int Width { get { return m_BackwardAStar.Width; } }
        public int Height { get { return m_BackwardAStar.Height; } }
        public TerrainCell[,] SearchSpace { get { return m_BackwardAStar.SearchSpace; } }

        public CooperativeAStar(TerrainCell[,] inSearchSpace, Int64 inCurrentCycle, Int64 inCycleResolution)
        {
            CycleResolution = inCycleResolution;
            CurrentCycle = inCurrentCycle;
            m_BackwardAStar = new Native_AStarSolver<TerrainCell>(inSearchSpace);
            m_IdleMap = new PathNodeKey[inSearchSpace.GetLength(0), inSearchSpace.GetLength(1)];
        }

        public void AddOneCycle()
        {
            CurrentCycle++;
        }

        public bool IsWalkable(Point inCell, int inMovableType)
        {
            return m_BackwardAStar.IsWalkable(inCell.X, inCell.Y, inMovableType);
        }

        private void ReservePath(Movable inMovable)
        {
            Int64 cycle = CurrentCycle / CycleResolution;

            foreach (var node in inMovable.Path)
            {
                for (int i = 0; i < inMovable.CycleSpeed; i++)
                {
                    MovablePathNode cell = m_Reservation[node.Key];

                    if (cell != null)
                    {
                        if (!inMovable.IsIdle)
                        {
                            if ((cell.Movable != inMovable) && cell.Movable.IsIdle)
                                throw new ApplicationException();
                        }
                        else
                            continue; // idles can only reserve free cells
                    }

                    node.Movable = inMovable;
                    m_Reservation[node.Key] = node;

                    cycle++;
                }
            }
        }

        internal Double Heuristic(bool inIsIdle, PathNodeKey inStart)
        {
            if (inIsIdle)
                return 0;

            return m_BackwardAStar.GetClosedGValue(new Point(inStart.X, inStart.Y));
        }

        internal virtual Double NeighborDistance(bool inIsIdle, PathNodeKey inStart, PathNodeKey inEnd)
        {
            int diffX = Math.Abs(inStart.X - inEnd.X);
            int diffY = Math.Abs(inStart.Y - inEnd.Y);

            switch (diffX + diffY)
            {
                case 1: return 1;
                case 2: return SQRT_2;
                case 0: return inIsIdle ? 0 : PauseCosts;
                default:
                    throw new ApplicationException();
            }
        }

        /// <summary>
        /// Calculates a cooperative path for the given movable and also initializes all
        /// required fields for it to walk this way. 
        /// TODO: Currently there is still an issue with idle settlers always walking through
        /// each other but I found no solution in a reasonable time and since it is not essential
        /// we will just defer it for now.
        /// </summary>
        internal void SetDynamicPath(
            Movable inMovable,
            Point inStart,
            Point inEnd,
            int inMovableType)
        {
            if ((inMovable.CycleSpeed < 1) || (inMovable.CycleSpeed > 1000))
                throw new ArgumentOutOfRangeException();

            Debug.Assert((CurrentCycle % CycleResolution) == 0);

            // search for current path position and remove outdated path nodes
            LinkedListNode<MovablePathNode> pathNodes = inMovable.Path.Last;
            LinkedListNode<MovablePathNode> currentPathNode = null;

            while (pathNodes != null)
            {
                LinkedListNode<MovablePathNode> prev = pathNodes.Previous;

                if (pathNodes.Value.Key.Time != CurrentCycle)
                {
                    inMovable.Path.Remove(pathNodes);

                    // if(!inMovable.IsIdle)
                    m_Reservation.Remove(pathNodes.Value.Key);
                }
                else
                {
                    currentPathNode = pathNodes;
                }

                pathNodes = prev;
            }

            Debug.Assert(currentPathNode != null, "Movable path does not contain a path node with current cycle time.");

            inMovable.Path.Clear();

            int PATH_PREDICTION = 5;
            var startNodeKey = currentPathNode.Value.Key;

            if (inMovable.IsIdle)
            {
                PATH_PREDICTION = 2;
            }

            try
            {
            RESTART:
                startNodeKey.Length = 0;

                // initialize backward search for heuristic
                m_BackwardAStar.BeginSearch(inEnd, inStart, inMovableType);

                // clear state
                m_ClosedSet.Clear();
                m_OpenSet.Clear();
                m_OrderedOpenSet.Clear();
                m_CameFrom.Clear();

                startNodeKey.G = 0;
                startNodeKey.H = Heuristic(inMovable.IsIdle, startNodeKey);
                startNodeKey.F = startNodeKey.H;

                m_OpenSet.Add(startNodeKey, null);
                m_OrderedOpenSet.Push(startNodeKey);

                // start search
                PathNodeKey[] neighborNodes = new PathNodeKey[9];

                while (m_OpenSet.Count > 0)
                {
                    PathNodeKey x = m_OrderedOpenSet.Pop();

                    if (x.Length >= PATH_PREDICTION)
                    {
                        ReconstructPath(inMovable, m_CameFrom[x]);

                        if (inMovable.IsIdle)
                        {
                            var firstNode = inMovable.Path.First.Value.Key;
                            inMovable.PathTarget = new Point(x.X, x.Y);
                            inMovable.PathSource = new Point(firstNode.X, firstNode.Y);

                            // substract 
                            foreach (var node in inMovable.Path)
                            {
                            }
                        }

                        inMovable.ReplanTime = inMovable.Path.Last.Value.Key.Time;
                        inMovable.CurrentNode = inMovable.Path.First;

                        return;
                    }

                    m_OpenSet.Remove(x);
                    m_ClosedSet.Add(x, null);

                    StoreNeighborNodes(x, neighborNodes, inMovable.CycleSpeed * CycleResolution);

                    for (int i = 0; i < neighborNodes.Length; i++)
                    {
                        PathNodeKey y = neighborNodes[i];
                        Boolean tentative_is_better;

                        if (y == null)
                            continue;

                        if (y.Time == 0)
                            continue;

                        if (!m_BackwardAStar.IsWalkable(y.X, y.Y, inMovableType))
                            continue;

                        if (m_ClosedSet.ContainsKey(y))
                            continue;

                        if (!CanWalkFromTo(inMovable, x, y))
                            continue;

                        Double tentative_g_score = x.G + NeighborDistance(inMovable.IsIdle, x, y);
                        Boolean wasAdded = false;

                        if (!m_OpenSet.ContainsKey(y))
                        {
                            m_OpenSet.Add(y, null);
                            tentative_is_better = true;
                            wasAdded = true;
                        }
                        else if (tentative_g_score < y.G)
                        {
                            tentative_is_better = true;
                        }
                        else
                        {
                            tentative_is_better = false;
                        }

                        if (tentative_is_better)
                        {
                            m_CameFrom.Add(y, x);

                            y.G = tentative_g_score;
                            y.H = Heuristic(inMovable.IsIdle, y);
                            y.F = y.G + y.H;

                            if (wasAdded)
                                m_OrderedOpenSet.Push(y);
                            else
                                m_OrderedOpenSet.Update(y);
                        }
                    }
                }

                /* 
                 * TODO: The following is something that isn't caught yet.
                 * Its a rare situation in which no "cooperative" path can be found. The simplest
                 * solution is to now search without cooperation which definitely will lead to
                 * movables walking through each other but keep in mind that by the time we get
                 * here there is NO other way out anyway, so what the heck...
                 * 
                 * Just to proof existence of this case imagine the following abstract but not impossible
                 * situation:
                 * 
                 * 1) There is a block of 10x10 settlers without any space between them. Now we start
                 *    at some border settler and remove 5 settlers in total by going from border to middle.
                 * 2) Now that we have such a "dead end" within this block, just imagine a settler walking
                 *    into it, directly towards the dead end and even over it. If the path planning time
                 *    is smaller than (fast guess) 10, the engine won't have a chance to catch this case
                 *    and by the time the settler walks into the dead end the dead end itself has no chance
                 *    to move out of way without walking over other settlers.
                 *    
                 * But this situation can happen also in "real" scenarios. 
                 */

                //goto RESTART;
                Debug.Assert(false, "No valid path could be found.");
            }
            finally
            {
            }
        }

        private void ReconstructPath(Movable inMovable, PathNodeKey current_node)
        {
            long time = current_node.Time;
            PathNodeKey lastNodeKey;

            m_CameFrom.GetPath(current_node, inMovable.Path);

            lastNodeKey = new PathNodeKey(current_node.X, current_node.Y, time);

            Direction? lastDirection = null;
            LinkedListNode<MovablePathNode> node = inMovable.Path.First;

            while (node != null)
            {
                LinkedListNode<MovablePathNode> nextNode = node.Next;

                if (nextNode != null)
                {
                    lastDirection = node.Value.Direction = GetWalkingDirection(node.Value.Key, nextNode.Value.Key);
                }
                else
                    node.Value.Direction = lastDirection;

                node = nextNode;
            }

            ReservePath(inMovable);
        }

        private Direction? GetWalkingDirection(PathNodeKey inFrom, PathNodeKey inTo)
        {
            int diffX = inTo.X - inFrom.X;
            int diffY = inTo.Y - inFrom.Y;

            if (diffX == 0)
            {
                if (diffY == -1) return Direction._000;
                else if (diffY == 1) return Direction._180;
                else return null;
            }
            else if (diffX == 1)
            {
                if (diffY == -1) return Direction._045;
                else if (diffY == 0) return Direction._090;
                else if (diffY == 1) return Direction._135;
                else return null;
            }
            else if (diffX == -1)
            {
                if (diffY == -1) return Direction._315;
                else if (diffY == 0) return Direction._270;
                else if (diffY == 1) return Direction._225;
                else return null;
            }
            else return null;
        }

        private void StoreNeighborNodes(PathNodeKey inAround, PathNodeKey[] inNeighbors, long inCycleSpeed)
        {
            int x = inAround.X;
            int y = inAround.Y;
            long t = inAround.Time + inCycleSpeed;

            if ((x > 0) && (y > 0))
                inNeighbors[0] = new PathNodeKey(x - 1, y - 1, t);
            else
                inNeighbors[0] = default(PathNodeKey);

            if (y > 0)
                inNeighbors[1] = new PathNodeKey(x, y - 1, t);
            else
                inNeighbors[1] = default(PathNodeKey);

            if ((x < Width - 1) && (y > 0))
                inNeighbors[2] = new PathNodeKey(x + 1, y - 1, t);
            else
                inNeighbors[2] = default(PathNodeKey);

            if (x > 0)
                inNeighbors[3] = new PathNodeKey(x - 1, y, t);
            else
                inNeighbors[3] = default(PathNodeKey);

            if (x < Width - 1)
                inNeighbors[4] = new PathNodeKey(x + 1, y, t);
            else
                inNeighbors[4] = default(PathNodeKey);

            if ((x > 0) && (y < Height - 1))
                inNeighbors[5] = new PathNodeKey(x - 1, y + 1, t);
            else
                inNeighbors[5] = default(PathNodeKey);

            if (y < Height - 1)
                inNeighbors[6] = new PathNodeKey(x, y + 1, t);
            else
                inNeighbors[6] = default(PathNodeKey);

            if ((x < Width - 1) && (y < Height - 1))
                inNeighbors[7] = new PathNodeKey(x + 1, y + 1, t);
            else
                inNeighbors[7] = default(PathNodeKey);

            inNeighbors[8] = new PathNodeKey(x, y, t); // must be the last one

            for (int i = 0; i < inNeighbors.Length; i++)
            {
                if (inNeighbors[i] != null)
                    inNeighbors[i].Length = inAround.Length + 1;
            }
        }

        private bool CanWalkFromTo(Movable inMovable, PathNodeKey inFrom, PathNodeKey inTo)
        {
            // TODO: when cycle speed is higher than one, we also need to check for reservations in between
            long currentTime = inFrom.Time;

            var toCell = m_Reservation[inTo];

            if ((toCell != null) && (inMovable != toCell.Movable))
            {
                if (!inMovable.IsIdle)
                    return false;

                if (toCell.Movable.IsIdle)
                    return false;
            }

            var fromCell = m_Reservation[new PathNodeKey(inFrom.X, inFrom.Y, currentTime + CycleResolution)];

            if (fromCell == null)
                return true;

            // check if collider originated from target
            var collider = fromCell.Movable;
            var collideCell = m_Reservation[new PathNodeKey(inTo.X, inTo.Y, currentTime)];

            if (collideCell == null)
                return true;

            if (collideCell.Movable == collider)
                return false;

            return true;
        }
    }
}
